#include <TFile.h>
#include <TRandom.h>
#include <TMath.h>
#include <TGeoManager.h>
#include <TEveGeoNode.h>
#include <TEveManager.h>
#include <TVector3.h>
#include <TGeoMatrix.h>

#include "CpLog.h"
#include "CpEvent.h"
#include "CpSimulator.h"

ClassImp(CpSimulator);

//________________________________________________________________________________________________________________
CpSimulator::CpSimulator(const char* name, const char* title) : TNamed(name, title) ,
   fNumberOfEvents(1),
   fNumberOfTracksInOneEvent(10),
   fCurrentFile(0),
   fOutputFileName("CpSim.root"),
   fGeometryFile("data/geometry/simple.root"),
   fTree("cpTree", "Coldpig Tree"),
   fEvent(0)
{
//
// Constructor
//

   CpDebug(CpLog::kDebug + 5, "<-");
   CpDebug(CpLog::kDebug + 5, "->");
}

//________________________________________________________________________________________________________________
CpSimulator::~CpSimulator()
{
//
// Destructor.
//

   CpDebug(CpLog::kDebug + 5, "<-");
   CpDebug(CpLog::kDebug + 5, "->");

}


Int_t CpSimulator::Init()
{
   CpDebug(CpLog::kDebug + 5, "<-");
   TGeoManager::Import(fGeometryFile.Data());
   if (!gGeoManager) return 1;

   fCurrentFile = TFile::Open(fOutputFileName.Data(), "RECREATE");
   fEvent = new CpEvent();
   fTree.Branch("events", "CpEvent", &fEvent);
   CpDebug(CpLog::kDebug + 5, "->");
   return 0;
}


Int_t CpSimulator::Finalize()
{
   CpDebug(CpLog::kDebug + 5, "<-");
   CpInfo(Form("Saving output file in %s ...", fCurrentFile->GetName()));
   fCurrentFile->cd();
   fTree.Write();
//   fTree.Print();
   fCurrentFile->Close();
   delete gGeoManager;

   CpDebug(CpLog::kDebug + 5, "->");
   return 0;
}


Int_t CpSimulator::Simulate()
{
   CpDebug(CpLog::kDebug + 5, "<-");
   if (Init()) return 1;

   TString vessel("VESSEL");
   Double_t step = 1e-2;
//   step=1;
   gGeoManager->SetStep(step);

   TRandom rnd(0);
//   Double_t delta=0.9;
//   Double_t delta2=1;
   const Double_t *xyz;
   TGeoNode*nextNode;
   const Double_t *currentDirection;
//   Double_t *normal;
   Double_t *currentNodeMatrix;
   Double_t divDot;

   TVector3 vCurrentDirection, vCurrentPoint;
   TVector3 vNormal;
   CpTrack *track;
   Int_t num = 0;
   for (Long64_t iEvent = 0; iEvent < fNumberOfEvents; iEvent++) {
      CpInfo(Form("=== Event %lld ===", iEvent));
      for (Long64_t iTrack = 0; iTrack < fNumberOfTracksInOneEvent; iTrack++) {
         CpInfo(Form("Track %lld", iTrack));
         track = fEvent->AddTrack(iTrack);
//       gGeoManager->InitTrack(rnd.Uniform(0, delta),rnd.Uniform(0, delta),-10,1,1,1);
//       gGeoManager->InitTrack(rnd.Uniform(0, delta),rnd.Uniform(0, delta),-10,rnd.Uniform(0, delta2),rnd.Uniform(0, delta2),1);
         gGeoManager->InitTrack(0, 0, -10, 1, 1, 1);
         while (!gGeoManager->IsOutside()) {
            nextNode = gGeoManager->Step(kFALSE);
            if (gGeoManager->IsOutside()) break;
//         CpInfo(Form("%s",nextNode->GetName()));
//         CpInfo(Form("%s",gGeoManager->GetPath()));
            TString nameTmp(nextNode->GetName());

            if (nameTmp.Contains(vessel)) {
               CpInfo(nameTmp.Data());
//           CpInfo(Form("IsEntering with normal"));
               gGeoManager->FindNextBoundary(step);
               CpInfo("1");
               currentDirection = gGeoManager->GetCurrentPoint();
               vCurrentPoint.SetXYZ(currentDirection[0], currentDirection[1], currentDirection[2]);
               vCurrentPoint.Print();
               currentDirection = gGeoManager->GetCurrentDirection();
               vCurrentDirection.SetXYZ(currentDirection[0], currentDirection[1], currentDirection[2]);
               vCurrentDirection.Print();

               currentDirection = gGeoManager->FindNormal();
//           currentDirection = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
               vNormal.SetXYZ(currentDirection[0], currentDirection[1], currentDirection[2]);
               vNormal.Print();


               TGeoHMatrix *matrix = gGeoManager->GetCurrentMatrix();
               currentNodeMatrix = matrix->GetTranslation();

               const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
               const Double_t *glob_dir = gGeoManager->GetCurrentDirection();
               const Double_t *glob_norm = gGeoManager->FindNormal();
//           const Double_t *glob_norm = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
               Double_t local_pt[3];
               Double_t local_dir[3];
               Double_t local_norm[3];
               gGeoManager->MasterToLocal(glob_pt, local_pt);
               gGeoManager->MasterToLocalVect(glob_dir, local_dir);
               gGeoManager->MasterToLocalVect(glob_norm, local_norm);
               CpInfo("2");
               vCurrentPoint.SetXYZ(local_pt[0], local_pt[1], local_pt[2]);
               vCurrentPoint.Print();
               vCurrentDirection.SetXYZ(local_dir[0], local_dir[1], local_dir[2]);
               vCurrentDirection.Print();
//           vNormal = vCurrentDirection.Orthogonal();
//           vNormal.SetXYZ(vCurrentDirection.Orthogonal().X(),vCurrentDirection.Orthogonal().X(),vCurrentDirection.Orthogonal().X());
               vNormal.SetXYZ(local_norm[0], local_norm[1], local_norm[2]);
               vNormal.Print();

               divDot = 2 * vCurrentDirection.Dot(vNormal) / vNormal.Dot(vNormal);
               CpInfo(Form("IsEntering divDot %f ", divDot));
               if (divDot == 0) {
                  vCurrentDirection *= -1;
               } else {
                  vCurrentDirection =  vCurrentDirection - vNormal * divDot;
               }
// u=v-2*n*(v,n)/(n,n)
//
               CpInfo(Form("IsEntering angle %f ", vCurrentDirection.Angle(vNormal) * 180 / TMath::Pi()));
//           vCurrentDirection.Rotate(TMath::Pi(),vNormal);
// vCurrentDirection.Rotate(TMath::Pi(),vNormal);

               local_dir[0] = vCurrentDirection.X();
               local_dir[1] = vCurrentDirection.Y();
               local_dir[2] = vCurrentDirection.Z();

//           local_dir[0] = -vCurrentDirection.Y();
//           local_dir[1] = -vCurrentDirection.Z();
//           local_dir[2] = -vCurrentDirection.X();


               vCurrentDirection.SetXYZ(local_dir[0], local_dir[1], local_dir[2]);
               vCurrentDirection.Print();


//           gGeoManager->FindNextBoundary(step);
// //           normal = gGeoManager->FindNormal();
//           vNormal.SetXYZ(normal[0],normal[1],normal[2]);
//           vNormal.Print();

//           matrix->ReflectX(kTRUE);
//           matrix->ReflectY(kTRUE);
//           matrix->ReflectZ(kTRUE);
//           matrix-> ReflectZ(kFALSE);
//           CpInfo(Form("IsEntering with currentNodeMatrix %f %f %f %d",currentNodeMatrix[0],currentNodeMatrix[1],currentNodeMatrix[2],matrix->IsReflection()));
//           gGeoManager->SetCurrentDirection(currentNodeMatrix[0],currentNodeMatrix[1],currentNodeMatrix[2]);
               Double_t glob_pt_new[3];
               Double_t glob_dir_new[3];
               gGeoManager->LocalToMaster(local_pt, glob_pt_new);
               gGeoManager->LocalToMasterVect(local_dir, glob_dir_new);
               CpInfo("3");

               vCurrentPoint.SetXYZ(glob_pt_new[0], glob_pt_new[1], glob_pt_new[2]);
               vCurrentPoint.Print();
               //           currentDirection = gGeoManager->GetCurrentDirection();
               vCurrentDirection.SetXYZ(glob_dir_new[0], glob_dir_new[1], glob_dir_new[2]);
               vCurrentDirection.Print();

               gGeoManager->SetCurrentDirection(glob_dir_new);

               /*
               //           gGeoManager->MasterToLocal(gGeoManager->GetCurrentPoint(),)
                         gGeoManager->MasterToLocalVect(gGeoManager->GetCurrentDirection(),currentNodeMatrix);
                         currentDirection = gGeoManager->GetCurrentDirection();
                         vCurrentDirection.SetXYZ(currentDirection[0],currentDirection[1],currentDirection[2]);
                         vCurrentDirection.Print();
                         vNormal.Print();
                         divDot = 2*vNormal.Dot(vCurrentDirection)/vNormal.Dot(vNormal);
                         CpInfo(Form("IsEntering divDot %f ",divDot));
                         vCurrentDirection = vCurrentDirection - vNormal*divDot;
                         vCurrentDirection.Print();
               //             vCurrentDirection.Rotate(TMath::Pi(),vNormal);
               //           vCurrentDirection *=-1;
               //           vCurrentDirection.RotateX(-1);
               //           vCurrentDirection.RotateY(vNormal.Y());
               //           vCurrentDirection.RotateZ(vNormal.Z());


               //           vCurrentDirection.Rotate(0,-vNormal);
               // vCurrentDirection.Rotate(vCurrentDirection.Angle(vNormal)+TMath::Pi(),vNormal);
                         CpInfo(Form("IsEntering angle %f ",vCurrentDirection.Angle(vNormal)*180/TMath::Pi()));
               //           vCurrentDirection.Rotate(3*TMath::Pi()/2,vNormal);
                         CpInfo(Form("IsEntering with currentDirection after %f %f %f",vCurrentDirection.X(),vCurrentDirection.Y(),vCurrentDirection.Z()));
               //           gGeoManager->SetCurrentDirection(-vCurrentDirection.X()*TMath::Cos(vNormal.X()),-vCurrentDirection.Y()*TMath::Cos(vNormal.Y()),vCurrentDirection.Z()*TMath::Cos(vNormal.Z()));

               //           vCurrentDirection.Dot(vNormal);
               //           gGeoManager->SetCurrentDirection(-vCurrentDirection.X(),-vCurrentDirection.Y(),vCurrentDirection.Z());
                         gGeoManager->SetCurrentDirection(vCurrentDirection.X(),vCurrentDirection.Y(),vCurrentDirection.Z());
               //           nextNode = gGeoManager->Step(kFALSE);
               //           gGeoManager->SetCurrentDirection(normal);
               //           break;*/

               num++;
               if (num > 10) break;

            }

            xyz = gGeoManager->GetCurrentPoint();
//         CpInfo(Form("%f %f %f",xyz[0],xyz[1],xyz[2]));
            track->AddPoint(xyz[0], xyz[1], xyz[2], 0.0);
         }

//       track->AddPoint(iTrack*0.1,iTrack*0.1,iTrack*1.0,0.0);
//       track->AddPoint(iTrack*0.1,iTrack*0.2,iTrack*2.0,1.0);
//       track->AddPoint(iTrack*0.1,iTrack*0.3,iTrack*3.0,2.0);
      }
      fTree.Fill();
      fEvent->Clear();
   }
   Finalize();
   CpDebug(CpLog::kDebug + 5, "->");
   return 0;
}


void CpSimulator::ShowEvent(const Long64_t& eventNum)
{
   CpDebug(CpLog::kDebug + 5, "<-");
   TGeoManager::Import(fGeometryFile.Data());
   if (!gGeoManager) return;

   gGeoManager->Print();

   CpDebug(CpLog::kDebug, Form("Geometry %s loaded ...", fGeometryFile.Data()));


   TEveManager::Create();

   CpDebug(CpLog::kDebug, "TEveManager::Create();");


   return ;
   TGeoNode* node = gGeoManager->GetTopNode();
   TEveGeoTopNode* en = new TEveGeoTopNode(gGeoManager, node);
   en->SetVisLevel(4);
   en->GetNode()->GetVolume()->SetVisibility(kFALSE);

   gEve->AddGlobalElement(en);

   gEve->Redraw3D(kTRUE);


   TFile *f = TFile::Open(fOutputFileName.Data(), "READ");
   TTree *tree = (TTree*) f->Get("cpTree");
   CpEvent *ev = 0;
//   tree->Print();
   tree->SetBranchAddress("events", &ev);
   tree->GetEntry(eventNum);
   ev->Print();





//   for(Long64_t iEvent=0;iEvent<fNumberOfEvents;iEvent++) {
//     tree->GetEntry(i);
//     ev->Print();
//   }
   CpDebug(CpLog::kDebug + 5, "->");
}

